#include "usb_task.h"

#include "cmsis_os.h"

#include "usb_device.h"
#include "usbd_cdc_if.h"
#include <stdio.h>
#include <stdarg.h>
#include "string.h"
#include <stdbool.h>

#include "main.h"

typedef struct
{
	uint8_t header;
	outgoing_msg_t msg; //payload 
	uint8_t crc8;
} __attribute__((packed)) USB_tx_packet_t; // USB transmit packet

USB_control_t USB_control; //USB communication struct

float feedback_yaw = 1.567574;
float feedback_pitch = 0.345663556;
float feedback_frequency = 2.5;
uint8_t feedback_auto_status = 0;

float control_yaw;
float control_pitch;
float control_frequency;

void usb_task(void const * argument)
{
	MX_USB_DEVICE_Init();
	USB_tx_packet_t packet;
	packet.header = 0x55;
		
	while(1)
	{
		usb_connection_judge();
		usb_get_transmit_value();
		packet.msg = USB_control.gimbal_feedback_data;
		packet.crc8 = crc8(&USB_control.gimbal_feedback_data, sizeof(outgoing_msg_t));		
		CDC_Transmit_FS((void *)&packet, sizeof(packet));
		osDelay(1);
	}
	
}

void usb_get_transmit_value(void)
{
	//TODO: set actual_value accroding to gimbal and shoot status
	USB_control.gimbal_feedback_data.gimbol_yaw = feedback_yaw * 1000000;
	USB_control.gimbal_feedback_data.gimbol_pitch = feedback_pitch * 1000000;
	USB_control.gimbal_feedback_data.shoot_status.state = 0;
	USB_control.gimbal_feedback_data.shoot_status.bullets_count = 5;
	USB_control.gimbal_feedback_data.shoot_status.frequency = feedback_frequency * 1000000;
	USB_control.gimbal_feedback_data.auto_status = feedback_auto_status;
}

uint8_t crc8(const void *vptr, int len)
{
	const uint8_t *data = (const uint8_t *)vptr;
	unsigned crc = 0;
	int i, j;
	for (j = len; j; j--, data++) 
	{
		crc ^= (*data << 8);
		for (i = 8; i; i--) 
		{
			if (crc & 0x8000)
				crc ^= (0x1070 << 3);
			crc <<= 1;
		}
	}
	return (uint8_t)(crc >> 8);
}

// returval: whether received a complete packet, 0 = fail, 1 = succeed
bool usb_receive_byte(uint8_t data_byte)
{
	uint8_t *msg_ptr = (uint8_t*)((void *)&(USB_control.gimbal_control_data));
	USB_control.receive_header = 0;
	USB_control.receive_payload= 0;
	USB_control.checksum_mismatch = 0;
	USB_control.checksum_matches = 0;
	if(USB_control.msg_len == -1)
	{
		if(MSG_HEADER == data_byte)
		{
			// received frame header
			USB_control.receive_header = 1;
			// start to receive payload
			USB_control.msg_len = 0; 
			return false;
		}
		else
		{
			// frame header not received, skipping
				return false;
		}
	}
	else					if(USB_control.msg_len < sizeof(incoming_msg_t))
								{
									// receiving payload
									USB_control.receive_payload= 1;
									msg_ptr[USB_control.msg_len++] = data_byte;
									return false;
								}
								else
																		{
																			// received crc8 checksum
																			uint8_t actual_crc8 = crc8(&USB_control.gimbal_control_data, sizeof(incoming_msg_t));
																			if (actual_crc8 == data_byte)
																			{
																				// checksum matches
																				USB_control.checksum_matches = 1;
																				// prepare for next frame
																				USB_control.msg_len = -1;
																				return true;
																			}
																			else
																			{
																				USB_control.checksum_mismatch = 1;
																				// checksum mismatch
																				for (size_t i = 0; i < sizeof(incoming_msg_t); i++)
																				{
																					if (msg_ptr[i] == MSG_HEADER)
																					{
																						USB_control.msg_len = sizeof(incoming_msg_t) - (i + 1);
																						memmove(msg_ptr, msg_ptr + (i + 1), USB_control.msg_len);
																						msg_ptr[USB_control.msg_len++] = data_byte;
																						return false;
																					}
																				}
																				if (data_byte == MSG_HEADER)
																				{
																					USB_control.msg_len = 0;
																					return false;
																				}
																				else
																				{
																					USB_control.msg_len = -1;
																					return false;
																			}
																			
																		}
																	}
}

void usb_connection_judge(void)
{
	if(USB_control.usb_watch_dog > 0)
		USB_control.usb_watch_dog--;
	
	if(USB_control.usb_watch_dog == 0)
		USB_control.USB_received = 0;
	else if(USB_control.usb_watch_dog > 0)
		USB_control.USB_received = 1;
}


void usb_instruction_handler(void)
{
	control_yaw = USB_control.gimbal_control_data.gimbol_yaw / 1000000.0;
	control_pitch = USB_control.gimbal_control_data.gimbol_pitch / 1000000.0;
	control_frequency = USB_control.gimbal_control_data.shoot_status.frequency / 1000000.0;
}

